#!/usr/bin/python3
import rospy
import cv2
import numpy as np
from drone_pkg.msg import Findring
import os

rospy.init_node('rinf_detect', anonymous=True)
pub = rospy.Publisher('/findring', Findring, queue_size=10)
msg = Findring()
cap = cv2.VideoCapture('/dev/came0')
kernel = np.ones((3,3), dtype=np.uint8)
rate = rospy.Rate(100)
os.system("gpio mode 19 out")
os.system("gpio write 19 1")

count = 0
led_flag = 0
while not rospy.is_shutdown():
    
    ret, img = cap.read()
    #img = cv2.resize(img, None, fx=0.5, fy=0.5)
    if ret:
        count+=1
        if (count >= 25):
            count = 0
            led_flag = 1 - led_flag
            if led_flag:
                os.system("gpio write 19 1")
            else:
                os.system("gpio write 19 0")

        img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
        
        img_gray = cv2.GaussianBlur(img_gray, (5,5), 0)
       # img_canny = cv2.Canny(img_gray, 64, 200)
        #img_dite = cv2.dilate(img_canny, kernel, 1)
       # img_dite = cv2.morphologyEx(img_canny, cv2.MORPH_CLOSE, kernel, iterations=1)
       # cv2.imshow('11', img_dite)
        # 检测圆形
        circles = cv2.HoughCircles(img_gray, cv2.HOUGH_GRADIENT, 1, minDist=200,
                                   param1=50, param2=60, minRadius=170, maxRadius=225)
        
       # 如果找到了圆形
        if circles is not None:
            circles = np.round(circles[0, :]).astype("int")  # 转换为整数
            
            # 找到最大圆形
            max_circle = max(circles, key=lambda c: c[2])  # 找到半径最大的圆形
            x, y, r = max_circle  # 提取最大圆形的参数
            msg.ring_x = x
            msg.ring_y = y
            msg.ring_r = r
            pub.publish(msg)
            # 画圆形
            cv2.circle(img, (x, y), r, (0, 255, 0), 4)  # 绘制圆形边界
            cv2.circle(img, (x, y), 5, (0, 0, 255), 10)  # 绘制圆心
            # 在圆心旁边显示坐标和半径
            text = f"({x}, {y}), r={r}"
            cv2.putText(img, text, (x + 10, y - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 0, 0), 2)

        cv2.imshow('1', img)

    if cv2.waitKey(1) & 0xFF == ord('q'):
        break
    rate.sleep()

cap.release()
cv2.destroyAllWindows()



